#include "stdafx.h"
#include "mathlib.h"

/*
inline void Slerp(float result[4], const float curr[4], const float next[4], float interpTime)
{
    // Calculate angle between them.
    float cosHalfTheta = curr[0] * next[0] + curr[1] * next[1] + curr[2] * next[2] + curr[3] * next[3];

    // if qa=qb or qa=-qb then theta = 0 and we can return qa
    if (abs(cosHalfTheta) >= 1.0)
    {
        result[0] = curr[0];
        result[1] = curr[1];
        result[2] = curr[2];
        result[3] = curr[3];
        return;
    }

    // Calculate temporary values.
    float halfTheta = acos(cosHalfTheta);
    float sinHalfTheta = sqrt(1.0f - cosHalfTheta * cosHalfTheta);

    // if theta = 180 degrees then result is not fully defined
    // we could rotate around any axis normal to qa or qb
    if (abs(sinHalfTheta) < 0.001)
    { // fabs is floating point absolute
        result[0] = curr[0] * 0.5f + next[0] * 0.5f;
        result[1] = curr[1] * 0.5f + next[1] * 0.5f;
        result[2] = curr[2] * 0.5f + next[2] * 0.5f;
        result[3] = curr[3] * 0.5f + next[3] * 0.5f;
        return;
    }

    float ratioA = sin((1 - interpTime) * halfTheta) / sinHalfTheta;
    float ratioB = sin(interpTime * halfTheta) / sinHalfTheta;
    //calculate Quaternion.
    result[0] = (curr[0] * ratioA + next[0] * ratioB);
    result[1] = (curr[1] * ratioA + next[1] * ratioB);
    result[2] = (curr[2] * ratioA + next[2] * ratioB);
    result[3] = (curr[3] * ratioA + next[3] * ratioB);
}
*/

void Slerp(float qt[4], const float p[4], float q[4], float t)
{
    int i;
    float omega, cosom, sinom, sclp, sclq;

    // decide if one of the quaternions is backwards
    float a = 0;
    float b = 0;
    for (i = 0; i < 4; i++)
    {
        a += (p[i]-q[i])*(p[i]-q[i]);
        b += (p[i]+q[i])*(p[i]+q[i]);
    }

    if (a > b)
    {
        for (i = 0; i < 4; i++)
        {
            q[i] = -q[i];
        }
    }

    cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];

    if ((1.0f + cosom) > 0.00000001)
    {
        if ((1.0f - cosom) > 0.00000001)
        {
            omega = acos(cosom);
            sinom = sin(omega);
            sclp = sin((1.0f - t)*omega) / sinom;
            sclq = sin(t*omega) / sinom;
        }
        else
        {
            sclp = 1.0f - t;
            sclq = t;
        }
        for (i = 0; i < 4; i++)
        {
            qt[i] = sclp * p[i] + sclq * q[i];
        }
    }
    else
    {
        qt[0] = -p[1];
        qt[1] = p[0];
        qt[2] = -p[3];
        qt[3] = p[2];
        sclp = sin((1.0f - t) * 0.5f * PI);
        sclq = sin(t * 0.5f * PI);
        for (i = 0; i < 3; i++)
        {
            qt[i] = sclp * p[i] + sclq * qt[i];
        }
    }
}

void QuaternionMatrix(float matrix[3][4], const float quat[4])
{
    matrix[0][0] = 1.0f - 2.0f * quat[1] * quat[1] - 2.0f * quat[2] * quat[2];
    matrix[1][0] = 2.0f * quat[0] * quat[1] + 2.0f * quat[3] * quat[2];
    matrix[2][0] = 2.0f * quat[0] * quat[2] - 2.0f * quat[3] * quat[1];

    matrix[0][1] = 2.0f * quat[0] * quat[1] - 2.0f * quat[3] * quat[2];
    matrix[1][1] = 1.0f - 2.0f * quat[0] * quat[0] - 2.0f * quat[2] * quat[2];
    matrix[2][1] = 2.0f * quat[1] * quat[2] + 2.0f * quat[3] * quat[0];

    matrix[0][2] = 2.0f * quat[0] * quat[2] + 2.0f * quat[3] * quat[1];
    matrix[1][2] = 2.0f * quat[1] * quat[2] - 2.0f * quat[3] * quat[0];
    matrix[2][2] = 1.0f - 2.0f * quat[0] * quat[0] - 2.0f * quat[1] * quat[1];
}

void R_ConcatTransforms (float out[3][4], const float in1[3][4], const float in2[3][4])
{
    out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0] +
        in1[0][2] * in2[2][0];
    out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1] +
        in1[0][2] * in2[2][1];
    out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2] +
        in1[0][2] * in2[2][2];
    out[0][3] = in1[0][0] * in2[0][3] + in1[0][1] * in2[1][3] +
        in1[0][2] * in2[2][3] + in1[0][3];
    out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0] +
        in1[1][2] * in2[2][0];
    out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1] +
        in1[1][2] * in2[2][1];
    out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2] +
        in1[1][2] * in2[2][2];
    out[1][3] = in1[1][0] * in2[0][3] + in1[1][1] * in2[1][3] +
        in1[1][2] * in2[2][3] + in1[1][3];
    out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0] +
        in1[2][2] * in2[2][0];
    out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1] +
        in1[2][2] * in2[2][1];
    out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2] +
        in1[2][2] * in2[2][2];
    out[2][3] = in1[2][0] * in2[0][3] + in1[2][1] * in2[1][3] +
        in1[2][2] * in2[2][3] + in1[2][3];
}


void MatrixMulV3(float out[3], const float in1[3], const float in2[3][4])
{
	out[0] = in2[0][0]*in1[0] + in2[0][1]*in1[1] + in2[0][2]*in1[2] + in2[0][3];
	out[1] = in2[1][0]*in1[0] + in2[1][1]*in1[1] + in2[1][2]*in1[2] + in2[1][3];
	out[2] = in2[2][0]*in1[0] + in2[2][1]*in1[1] + in2[2][2]*in1[2] + in2[2][3];
}

void QuaternionMultiply(float out[4], const float in1[4], const float in2[4])
{
	// To multiply a quaternion you must first do the dot and cross product
	// of the 2 quaternions then add/subract them to a result.
	out[0] = in1[3] * in2[0] + in1[0] * in2[3] + in1[1] * in2[2] - in1[2] * in2[1];
	out[1] = in1[3] * in2[1] - in1[0] * in2[2] + in1[1] * in2[3] + in1[2] * in2[0];
	out[2] = in1[3] * in2[2] + in1[0] * in2[1] - in1[1] * in2[0] + in1[2] * in2[3];
	out[3] = in1[3] * in2[3] - in1[0] * in2[0] - in1[1] * in2[1] - in1[2] * in2[2];
}

void MuBmdQuaternionFromRotate(float out[4], const float vRotate[3])
{
	float vx=vRotate[0] * 0.5f;
	float vy=vRotate[1] * 0.5f;
	float vz=vRotate[2] * 0.5f;

	float q1[4]={sin(vx),0,0,cos(vx)};
	float q2[4]={0,sin(vy),0,cos(vy)};
	float q3[4]={0,0,sin(vz),cos(vz)};

	float temp[4];
    QuaternionMultiply(temp,q1,q2);
	QuaternionMultiply(out,temp,q3);
}

void MuBmdQuaternionRotateMatrix(float matrix[3][4], const float quat[4])
{
	float fTx  = quat[0]*2.0f;
	float fTy  = quat[1]*2.0f;
	float fTz  = quat[2]*2.0f;
	float fTwx = fTx*quat[3];
	float fTwy = fTy*quat[3];
	float fTwz = fTz*quat[3];
	float fTxx = fTx*quat[0];
	float fTxy = fTy*quat[0];
	float fTxz = fTz*quat[0];
	float fTyy = fTy*quat[1];
	float fTyz = fTz*quat[1];
	float fTzz = fTz*quat[2];

	matrix[0][0] = 1.0f-(fTyy+fTzz);
	matrix[0][1] = fTxy+fTwz;
	matrix[0][2] = fTxz-fTwy;
	matrix[1][0] = fTxy-fTwz;
	matrix[1][1] = 1.0f-(fTxx+fTzz);
	matrix[1][2] = fTyz+fTwx;
	matrix[2][0] = fTxz+fTwy;
	matrix[2][1] = fTyz-fTwx;
	matrix[2][2] = 1.0f-(fTxx+fTyy);
	matrix[0][3] = matrix[1][3] = matrix[2][3] = 0.0f;
}
